/*
 * 
 * This is the Whomper robot, created by Sean Jeronimo of Team 1425.
 * This robot implements the functionality of Team 1425's 2007 robot for the game "Rack 'n' Roll."
 * 
 * A note about conventions: it is normally not conventional for Team 1425 to do things such as
 * placing a leading 'k' before a constant, or capitalizing the first letter of a method; however,
 * in the spirit of maintaining consistency with the base class, these conventions are followed.
 * It is probable that, given the team, Team 1425 will go through the WPILib and standardize it
 * to meet their conventional preferences.
 * 
 * Notice, there're no headerguards. Maybe I'm being a terrible programmer in assuming that nobody
 * will include this file but me. >_<!
 *  
 */

#include "IterativeRobot.h"
#if 0
#include "SwitchListener.h"
#endif
#include "Joystick.h"
#include "Switch.h"
#include "TwoPositionSwitch.h"
#include "ThreePositionSwitch.h"
#include "Compressor.h"
#include "RobotDrive.h"
#include "Lift.h"
#include "Grabber.h"
#include "LED.h"

class Whomper : public IterativeRobot, public SwitchListener
{		
public:
	Whomper(void);
	
	void DisabledInit(void);
	void AutonomousInit(void);
	void TeleopInit(void);
	
	void DisabledPeriodic(void);
	void AutonomousPeriodic(void);
	void TeleopPeriodic(void);

	void DisabledContinuous(void);
	void AutonomousContinuous(void);
	void TeleopContinuous(void);
	
	void SwitchChanged(int id);
	
private:
#if 0
	static const int kNeutralLEDNumber	     = 0;
	static const int kHighPlaceLEDNumber	 = 1;
	static const int kMidPlaceLEDNumber		 = 2;
	static const int kLowPlaceLEDNumber		 = 3;
	static const int kTravelLEDNumber		 = 4;
	static const int kReadyLEDNumber		 = 5;
	static const int kHighRemoveLEDNumber	 = 6;
	static const int kMidRemoveLEDNumber	 = 7;
	static const int kLowRemoveLEDNumber	 = 8;
	static const int kGroundLEDNumber		 = 9;
	
	static const int kNeutralLEDChannel		 = -1;
	static const int kHighPlaceLEDChannel	 = -1;
	static const int kMidPlaceLEDChannel	 = -1;
	static const int kLowPlaceLEDChannel	 = -1;
	static const int kTravelLEDChannel		 = -1;
	static const int kReadyLEDChannel		 = -1;
	static const int kHighRemoveLEDChannel	 = -1;
	static const int kMidRemoveLEDChannel	 = -1;
	static const int kLowRemoveLEDChannel	 = -1;
	static const int kGroundLEDChannel		 = -1;
#endif
	
	static const int kHighPlacePositionNumber  = 0;
	static const int kMidPlacePositionNumber   = 1;
	static const int kLowPlacePositionNumber   = 2;
	static const int kTravelPositionNumber     = 3;
	static const int kHighRemovePositionNumber = 4;
	static const int kMidRemovePositionNumber  = 5;
	static const int kLowRemovePositionNumber  = 6;
	static const int kGroundPositionNumber     = 7;
	
	static const int kLeftJoystickChannel    = 1;
	static const int kRightJoystickChannel   = 2;
	static const int kArmJoystickChannel     = 3;

	static const int kGrabberSwitchChannel   = -1; //TODO: set these
	static const int kTeachKeyChannel        = -1;	
#if 0
	static const int kHighSwitchChannel      = -1;
	static const int kMedSwitchChannel       = -1;
	static const int kLowSwitchChannel       = -1;
	static const int kTravelSwitchChannel    = -1;
	
	static const int kHighSwitchNumber      = 0;
	static const int kMedSwitchNumber       = 1;
	static const int kLowSwitchNumber       = 2;
	static const int kTravelSwitchNumber    = 3;
#endif
	
	static const int kGrabberSwitchNumber   = 4;
	static const int kTeachKeyNumber        = 5;
	
	static const int kPressureSwitchChannel  = 14;
	static const int kCompressorChannel		 = 2;
	static const int kDriveLeftMotorChannel	 = 2;
	static const int kDriveRightMotorChannel = 1;
	static const int kLiftLeftMotorChannel   = 3;
	static const int kLiftRightMotorChannel  = 4;
	static const int kLiftHeightChannel 	 = 1;
	static const int kGrabberChannel         = 2;
	
	static const float kDriveNeutralSpeed	 = 0.0f;
	static const int   kArmDoNotMove		 = -1;
	
	//static const int kLEDCount = 10;
	//LED *m_LEDs[kLEDCount];
	
	static const int kArmPositionCount = 8;
	int m_armPositions[kArmPositionCount];
	int m_armTarget;
	
	static const int kSwitchCount = 2;//6;
	Switch *m_switches[kSwitchCount];
	
	//Input
	Joystick            *m_leftJoystick;
	Joystick            *m_rightJoystick;
	Joystick            *m_armJoystick;
	
	//Output
	Compressor *m_compressor;
	RobotDrive *m_driveTrain;
	Lift       *m_lift;
	Grabber    *m_grabber;
	
	//Noput
	int m_disabled_periodic_loop_count;
	int m_auto_periodic_loop_count;
	int m_tele_periodic_loop_count;
	
	void setExclusiveLED(LED *led);
	void setAllLEDs(bool shouldBeOn);
	int  getLiftTarget(void);
	void doTeachMode(void);
};

START_ROBOT_CLASS(Whomper);
